
#include "MEC_ESC.h"
#include "Arduino.h"

#define ESC_MOTOR_OFF 1000
#define ESC_MOTOR_FULL 2000
#define ESC_MOTOR_ADJ_START 1090 
#define ESC_MOTOR_ADJ_FULL 1890 


MEC_ESC::MEC_ESC(int pin)
{
	speed = 0;
	this->pin = pin;
	
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


void MEC_ESC::initESC()
{
	motor.attach(pin);  
	delay(100);
	motor.writeMicroseconds(ESC_MOTOR_OFF);
	delay(100);	
}

void MEC_ESC::setSpeed(float speed)
{	
	this->speed = speed;
	motor.writeMicroseconds((int)mapfloat(speed, 0, PI, ESC_MOTOR_ADJ_START, ESC_MOTOR_ADJ_FULL));
	
}

void MEC_ESC::calibrate()
{
	motor.writeMicroseconds(ESC_MOTOR_FULL);  
	delay(3000);
	motor.writeMicroseconds(ESC_MOTOR_OFF); 
}


